#include "STMGood.h"
#include "stdio.h"

//-----------------------------------???????????????��?????????
int Count=0;
char Cmd1[20];
char Cmd2[80];
char Data[100];
char Str1[100];
double xx[100];
//-----------------------------------?????????????????????????

char m;

//------------------------------------

float P = 0.0f;
float I = 0.0f;
float D = 0.0f;
float S1 = 0.0f;
float S2 = 0.0f;
float p = 0.0f;
float i =0.0f;
float d = 0.0f;
double A = 0.0f;
double B = 0.0f;
double C = 0.0f;
double X = 0.0f;
double Y = 0.0f;

//-------------------------------------------------------------------------------
int cmd(char *Cmd,int n)
{
  //??????????��????????
  
  if(CompStr(Cmd,"q"))
  {
    p += 0.1f;
		printf("pp=%f  ii=%f  dd=%f\r\n",p,i,d);
    return 0;
  }
//	if(CompStr(Cmd,"q"))
//  { 
//		printf("outerr=%f\r\n",StirMotorPIDout_Right.errNow);
//		printf("innererr=%f\r\n",StirMotorPIDinner.errNow);
//		printf("nowpostion=%d\r\n",StirMotorcontrol_Right.nowpos);
//    printf("postion=%d\r\n",StirMotorcontrol_Right.backspeed);
//		printf("postion=%d\r\n",StirMotorcontrol_Right.init);
//    return 0;
//  }
  if(CompStr(Cmd,"w"))
  {
    i += 0.001f;
		printf("pp=%f  ii=%f  dd=%f\r\n",p,i,d);
//		Remote_RotatePos+=20000;
//		Remote_LiftPos+=40000;

		//Left_Servo_Pos -= 50;
		//targetposition_Left += 1000;
		//printf("Left_Servo_Pos=%d\r\n",Left_Servo_Pos);
    return 0;
  }
  if(CompStr(Cmd,"e"))
  {
    d += 0.1f;
		printf("pp=%f  ii=%f  dd=%f\r\n",p,i,d);
    return 0;
  }
  if(CompStr(Cmd,"a"))
  {
    p -= 0.1f;
		printf("pp=%f  ii=%f  dd=%f\r\n",p,i,d);
    return 0;
  }
  if(CompStr(Cmd,"s"))
  {
    i -= 0.001f;
		printf("pp=%f  ii=%f  dd=%f\r\n",p,i,d);
    return 0;
  }
  if(CompStr(Cmd,"d"))
  {
    d -= 0.1f;
		printf("pp=%f  ii=%f  dd=%f\r\n",p,i,d);
    return 0;
  }
	if(CompStr(Cmd,"r"))
  {
    p = 0.0;
		i = 0.0;
		d = 0.0;
		printf("pp=%f  ii=%f  dd=%f\r\n",p,i,d);
    return 0;
  }
	
	if(CompStr(Cmd,"u"))
  {
    P = P + 0.1f;
		printf("P=%f  I=%f  D=%f\r\n",P,I,D);
    return 0;
  }
	if(CompStr(Cmd,"i"))
  {
    I = I + 0.001f;
		printf("P=%f  I=%f  D=%f\r\n",P,I,D);
    return 0;
  }
	if(CompStr(Cmd,"o"))
  {
    D = D + 0.1f;
		printf("P=%f  I=%f  D=%f\r\n",P,I,D);
    return 0;
  }
	if(CompStr(Cmd,"j"))
  {
    P = P - 0.1f;
		printf("P=%f  I=%f  D=%f\r\n",P,I,D);
    return 0;
  }
	if(CompStr(Cmd,"k"))
  {
    I = I - 0.001f;
		printf("P=%f  I=%f  D=%f\r\n",P,I,D);
    return 0;
  }
	if(CompStr(Cmd,"l"))
  {
    D = D - 0.1f;
		printf("P=%f  I=%f  D=%f\r\n",P,I,D);
    return 0;
  }
	if(CompStr(Cmd,"p"))
  {
    P = 0.0;
		I = 0.0;
		D = 0.0;
		printf("P=%f  I=%f  D=%f\r\n",P,I,D);
    return 0;
  }
	
	return 0;
}


void multi1(int n)
{
	p = xx[1];
	i = xx[2];
	d = xx[3];
	S1 = xx[4];
	printf("pp=%.2f  ii=%.2f  dd=%.2f  Target=%.2f\r\n",p,i,d,S1);
}
void multi2(int n)
{
	P = xx[1];
	I = xx[2];
	D = xx[3];
	S2 = xx[4];
	printf("P=%f  I=%f  D=%f  S2=%f\r\n",P,I,D,S2);
}
void multi3(int n)
{
	A = xx[1];
	B = xx[2];
	C = xx[3];
	printf("A=%f  B=%f  C=%f\r\n",A,B,C);
}
void multi4(int n)
{
	X = xx[1];
	Y = xx[2];
	printf("X=%f  Y=%f\r\n",X,Y);
}


int Command(char *Cmd,int n)
{
//------------------------------????????????-----------------------------
	if(CompStr(Cmd,"*1"))
	{
		multi1(n);
		return 0;
	}
	if(CompStr(Cmd,"*2"))
	{
		multi2(n);
		return 0;
	}
	if(CompStr(Cmd,"*3"))
	{
		multi3(n);
		return 0;
	}
	if(CompStr(Cmd,"*4"))
	{
		multi4(n);
		return 0;
	}

  return 0;
  //------------------------------------------
}


//--------------------------------------------??????????????????????-------------------------

void Dealdata(int Rx)                  //??????��???????????????
{
	int i;
	if (Rx=='(' && Count==0)
	{
		Data[Count++]=Rx;
    }
	else if (Count>0)
	{
		Data[Count++]=Rx;
    }
	if (Rx==')')
	{
		Data[Count]='\0';
		Count=0;
	for (i=0;i<Strlen(Data)-1;i++)
	{
		Data[i]=Data[i+1];
    }
	Data[Strlen(Data)-2]='\0';
	SplitStr(Data, Cmd1, Cmd2);
	Command(Cmd1, DealStr(Cmd2));
	for(i=0;i<80;i++)
	{
		Cmd2[i] = 0;
	}
    }
}

//-----------------------------------------------

int DealStr(char *Str)
{
	int i;
	int m=0;
	int n=1;
	int len=Strlen(Str);
	if(len==0)
	{
		return 0;
  }
	for(i=0;i<len;i++)
	{
		if (CompStr(SubStr(Str,i,i+1)," "))
		{
			xx[n]=StrToFloat(SubStr(Str,m,i));
			n++;
			m=i+1;
    }
		else
		{
			if(len-i==1)
			{
				xx[n]=StrToFloat(SubStr(Str,m,len));
      }
			continue;
    }
  }
	return n;
}

char *SubStr(char *Str,int start,int final)            //??????????
{
	int i;
	for(i=start;i<final;i++)
	{
		*(Str1+i-start)=*(Str+i);
  }
	*(Str1+final-start)='\0';
	return Str1;
}

void SplitStr(char *Str,char *Str1,char *Str2)           //????????
{
	int i;
	int k=FirstSpace(Str);
	int len=Strlen(Str);
	for (i=0;i<k;i++)
	{
		Str1[i]=Str[i];
	}
	Str1[k]='\0';
	for (i=k+1;i<len;i++)
	{
		Str2[i-k-1]=Str[i];
	}
	Str2[len]='\0';
	if(k==-1)
	{
		CopyStr(Str,Str1);
		Str2[0]='\0';
	}
}

int FirstSpace(char *Str)                             //?????????��???????��??
{
	int i;
	for (i=0;i<100;i++)
	{
		if (Str[i]==' ')
		{
			return i;
    }
		else if(Str[i]=='\0')
		{
			return -1;
    }
	}
	return -1;
}

int Strlen(char *Str)                                 //????????????
{
	int len=0;
	while(*(Str+len++)!='\0');
return --len;
}

int CompStr(char *Str1,char *Str2)                     //??????????????????
{
	int i;
	int aStr=Strlen(Str1);
	int bStr=Strlen(Str2);
	if(aStr==bStr)
	{
		for (i=0;i<aStr;i++)
		{
			if(*(Str1+i)==*(Str2+i))
				continue;
			else
				return 0;
    }
		return 1;
	}
	else
	{
		return 0;
	}
}

int CopyStr(char *Str1,char *Str2)                      //???????????????????Str1?????Str2
{
	int i;
	for (i=0;i<100;i++)
	{
		if(Str1[i]!='\0')
		{
			Str2[i]=Str1[i];
    }
		else
		{
			Str2[i]=Str1[i];
			return 1;
    }
}
	return 1;
}

int StrToInt(char *Str)                                 //?????????????
{
	return (int)StrToFloat(Str);
}

float StrToFloat(char *Str)                              //?????????��??
{
    int i,j,k,negative=0;
    #define s_temp Str
    double result=0,result_1=0;
    for(i=0;i<10;i++)
    {
		j=Str[i];
		if(j==0||((j<'0'||j>'9')&&(j!='.')&&(j!='-')))
			break;
    }
    k=j=i;
    for(i=0;i<j;i++)
    {
        if(s_temp[i]=='.')
				break;
    }

    for(j=0;j<i;j++)
    {
        if(s_temp[j]=='-')
        {
			negative=1;
            continue;
        }
        result=result*10+(s_temp[j]-'0');
    }
    j++;
    i=j;
    for(;j<k;j++)
    {
        if(s_temp[j]<'0'||s_temp[j]>'9')
			break;
        result_1=result_1*10+(s_temp[j]-'0');
    }
    for(j=0;j<(k-i);j++)
		result_1*=0.1;
    result+=result_1;

    if(negative)result=-result;
    return result;
}
